#include "ulog_parser.h"

// save all sensors in sorted multimap
template <typename T>
void ULogParser::AddSensorsToMap(const std::vector<T>& sensors,
                                 const SensorType& type) {
  type_idx_s type_idx;
  type_idx.type = type;
  for (size_t i = 0; i < sensors.size(); i++) {
    type_idx.idx = i;
    all_sensors_map_.emplace(sensors[i].timestamp, type_idx);
  }
}

bool ULogParser::LoadULogMsgs() {
  ReadFileHeader(ulog_fin_);
  ReadFileDefinitions(ulog_fin_);

  // loaded all sensors
  ReadDatas(ulog_fin_);
  LOG(INFO) << "accel size: " << sensor_accels_.size();
  LOG(INFO) << "gyro  size: " << sensor_gyros_.size();
  LOG(INFO) << "mag   size: " << sensor_mags_.size();
  LOG(INFO) << "baro  size: " << sensor_baros_.size();
  LOG(INFO) << "gps size: " << sensor_gpss_.size();
  LOG(INFO) << "combined size: " << sensor_combineds_.size();

  // add all sensors
  AddSensorsToMap(sensor_accels_, SensorType::ACCEL);
  AddSensorsToMap(sensor_gyros_, SensorType::GYRO);
  AddSensorsToMap(sensor_mags_, SensorType::MAG);
  AddSensorsToMap(sensor_baros_, SensorType::BARO);
  AddSensorsToMap(sensor_gpss_, SensorType::GPS);
  AddSensorsToMap(sensor_combineds_, SensorType::COMBINED);
  LOG(INFO) << "all sensors size: " << all_sensors_map_.size();

  return true;
}

bool ULogParser::ReadFileHeader(std::ifstream& fin) {
  fin.seekg(0);
  ulog_file_header_s msg_header;
  fin.read((char*)&msg_header, sizeof(msg_header));

  if (!fin) {
    return false;
  }

  // LOG(INFO) << "file ts: " << msg_header.timestamp
  //           << ", magic: " << msg_header.magic;
  file_start_time_ = msg_header.timestamp;

  // verify it's an ULog file
  char magic[8];
  magic[0] = 'U';
  magic[1] = 'L';
  magic[2] = 'o';
  magic[3] = 'g';
  magic[4] = 0x01;
  magic[5] = 0x12;
  magic[6] = 0x35;
  return std::memcmp(magic, msg_header.magic, 7) == 0;
}

bool ULogParser::ReadFileDefinitions(std::ifstream& fin) {
  ulog_message_header_s message_header;
  fin.seekg(sizeof(ulog_file_header_s));

  while (true) {
    fin.read((char*)&message_header, ULOG_MSG_HEADER_LEN);
    if (!fin) {
      return false;
    }
    // LOG(INFO) << "msg type: " << message_header.msg_type
    //           << ", msg size: " << message_header.msg_size;

    switch (message_header.msg_type) {
      case (int)ULogMessageType::FLAG_BITS:
        if (!ReadFlagBits(fin, message_header.msg_size)) {
          return false;
        }
        break;
      case (int)ULogMessageType::FORMAT:
        if (!ReadFormat(fin, message_header.msg_size)) {
          return false;
        }
        break;
      case (int)ULogMessageType::PARAMETER:
        if (!ReadParams(fin, message_header.msg_size)) {
          return false;
        }
        break;
      case (int)ULogMessageType::ADD_LOGGED_MSG:
        data_section_start_ = fin.tellg() - (std::streamoff)ULOG_MSG_HEADER_LEN;
        return true;
      case (int)ULogMessageType::INFO:
      case (int)ULogMessageType::INFO_MULTIPLE:
      case (int)ULogMessageType::PARAMETER_DEFAULT:
        fin.seekg(message_header.msg_size, std::ios::cur);
        break;
      default:
        LOG(INFO) << "unknow log definition type: "
                  << (int)message_header.msg_type
                  << " , size: " << (int)message_header.msg_size
                  << ", offset: " << (int)fin.tellg();
        fin.seekg(message_header.msg_size, std::ios::cur);
        break;
    }
  }
  return true;
}

bool ULogParser::ReadFormat(std::ifstream& fin, uint16_t msg_size) {
  read_buffer_.reserve(msg_size + 1);
  char* format_ptr = (char*)read_buffer_.data();
  fin.read(format_ptr, msg_size);
  format_ptr[msg_size] = 0;

  if (!fin) {
    return false;
  }

  std::string str_format(format_ptr);
  size_t pos = str_format.find(':');
  if (pos == std::string::npos) {
    return false;
  }

  std::string name = str_format.substr(0, pos);
  std::string fields = str_format.substr(pos + 1);
  file_formats_[name] = fields;

  return true;
}

bool ULogParser::ReadFlagBits(std::ifstream& fin, uint16_t msg_size) {
  if (msg_size != 40) {
    LOG(INFO) << "unsupported message length for FLAG_BITS message size: "
              << msg_size;
    return false;
  }

  read_buffer_.reserve(msg_size);
  uint8_t* msg_ptr = (uint8_t*)read_buffer_.data();
  fin.read((char*)msg_ptr, msg_size);
  uint8_t* incompat_flags = msg_ptr + 8;

  // handle & validate the flags
  bool contains_appended_data =
      incompat_flags[0] & ULOG_INCOMPAT_FLAG0_DATA_APPENDED_MASK;
  bool has_unknow_incompats_bits = false;

  if (incompat_flags[0] & ~0x1) {
    has_unknow_incompats_bits = true;
  }
  for (int i = 1; i < 8; i++) {
    if (incompat_flags[i]) {
      has_unknow_incompats_bits = true;
    }
  }
  if (has_unknow_incompats_bits) {
    LOG(INFO) << "Log contains unknow incompat bit set. Refusing to parse";
    return false;
  }
  if (contains_appended_data) {
    uint64_t appended_offsets[3];
    std::memcpy(appended_offsets, msg_ptr + 16, sizeof(appended_offsets));
    if (appended_offsets[0] > 0) {
      LOG(INFO) << "Log contains appended data, Replay will ignore this data";
      read_until_file_position_ = appended_offsets[0];
    }
  }
  return true;
}

bool ULogParser::ReadParams(std::ifstream& fin, uint16_t msg_size) {
  read_buffer_.reserve(msg_size);
  uint8_t* msg_ptr = (uint8_t*)read_buffer_.data();
  fin.read((char*)msg_ptr, msg_size);

  if (!fin) {
    return false;
  }

  uint8_t key_len = msg_ptr[0];
  std::string key((char*)msg_ptr + 1, key_len);

  size_t pos = key.find(' ');
  if (pos == std::string::npos) {
    return false;
  }

  std::string type = key.substr(0, pos);
  std::string param_name = key.substr(pos + 1);

  // LOG(INFO) << "param type: " << type << ", name: " << param_name;

  if (type != "int32_t" && type != "float") {
    LOG(INFO) << "unknown parameter type: " << type << ", name: " << param_name;
    return true;
  }

  // TODO(llhuang): current ignore all params
  return true;
}

bool ULogParser::ReadDatas(std::ifstream& fin) {
  LOG(INFO) << "load msgs";
  /**
   * we know the next message must be an ADD_LOGGED_MSG
   */
  fin.seekg(data_section_start_);
  while (fin.good()) {
    // msg header
    ulog_message_header_s message_header;
    fin.read((char*)&message_header, ULOG_MSG_HEADER_LEN);
    // LOG(INFO) << "msg type: " << message_header.msg_type
    //           << ", msg size: " << message_header.msg_size;

    if (!fin) {
      return false;
    }

    switch (message_header.msg_type) {
      case (int)ULogMessageType::ADD_LOGGED_MSG: {
        // read ADD_LOGGED_MSG
        read_buffer_.reserve(message_header.msg_size + 1);
        char* msg_ptr = (char*)read_buffer_.data();
        fin.read(msg_ptr, message_header.msg_size);
        msg_ptr[message_header.msg_size] = 0;

        uint8_t multi_id = *(uint8_t*)msg_ptr;
        uint16_t msg_id = ((uint8_t)msg_ptr[1]) | (((uint8_t)msg_ptr[2]) << 8);
        std::string topic_name(msg_ptr + 3);

        // LOG(INFO) << "topic: " << topic_name
        //           << ", multi id: " << (int16_t)multi_id
        //           << ", msg id: " << msg_id;
        break;
      }
      case (int)ULogMessageType::DATA: {
        uint16_t file_msg_id;
        fin.read((char*)&file_msg_id, sizeof(file_msg_id));
        switch (file_msg_id) {
          // accel
          case 142: {
            sensor_accel_s sensor_accel;
            fin.read((char*)&sensor_accel,
                     message_header.msg_size - sizeof(file_msg_id));
            sensor_accels_.push_back(sensor_accel);
            break;
          }
            // gyro
          case 148: {
            sensor_gyro_s sensor_gyro;
            fin.read((char*)&sensor_gyro,
                     message_header.msg_size - sizeof(file_msg_id));
            sensor_gyros_.push_back(sensor_gyro);
            break;
          }
            // mag
          case 151: {
            sensor_mag_s sensor_mag;
            fin.read((char*)&sensor_mag,
                     message_header.msg_size - sizeof(file_msg_id));
            sensor_mags_.push_back(sensor_mag);
            break;
          }
            // sensor combined
          case 11: {
            sensor_combined_s sensor_combined;
            fin.read((char*)&sensor_combined,
                     message_header.msg_size - sizeof(file_msg_id));
            sensor_combineds_.push_back(sensor_combined);
            break;
          }
            // baro
          case 145: {
            sensor_baro_s sensor_baro;
            fin.read((char*)&sensor_baro,
                     message_header.msg_size - sizeof(file_msg_id));
            sensor_baros_.push_back(sensor_baro);
            break;
          }
          case 147: {
            sensor_gps_s sensor_gps;
            fin.read((char*)&sensor_gps,
                     message_header.msg_size - sizeof(file_msg_id));
            sensor_gpss_.push_back(sensor_gps);
            break;
          }
          default:
            fin.seekg(message_header.msg_size - sizeof(file_msg_id),
                      std::ios::cur);
            break;
        }
        break;
      }
      case (int)ULogMessageType::REMOVE_LOGGED_MSG:
      case (int)ULogMessageType::DROPOUT:
      case (int)ULogMessageType::INFO:
      case (int)ULogMessageType::INFO_MULTIPLE:
      case (int)ULogMessageType::SYNC:
      case (int)ULogMessageType::LOGGING:
      case (int)ULogMessageType::PARAMETER_DEFAULT:
        fin.seekg(message_header.msg_size, std::ios::cur);
        break;
      default:
        break;
    }
  }
}
